//#include"OutputHandler.cpp"
//#include"Virtual Coordinate Processor.cpp"
#include <ctime>
#include <cmath>
#include "Serial.h"
#include <iostream>

using namespace std;
class OutputHandler
{
private:
	Serial* Arduino;
	char buffer[11];

public:
	OutputHandler()
	{
		Arduino = new Serial("COM3"); // Look in device manager to find the correct COM port
	}
	void setMode(int Mode)
	{
		//use fixed width buffer
		buffer[0] = '0';
		buffer[1] = Mode + 48; // convert to char
		buffer[2] = '0';
		buffer[3] = '0';
		buffer[4] = '0';
		buffer[5] = '0';
		buffer[6] = '0';
		buffer[7] = '0';
		buffer[8] = '0';
		buffer[9] = '0';
		buffer[10] = '0';
		Arduino->WriteData(buffer,11);
	}
	void SendPWM(float xPWM,float yPWM) // not yet implemented. waiting on motors 
	{

		sprintf(buffer,"2%1.2f,%1.2f0",xPWM,yPWM);
		printf(buffer);
		printf("\n");
		Arduino->WriteData(buffer,11);
	}
	void SendLaserOnOff(int state)
	{
		//use fixed width buffer
		buffer[0] = '1';
		buffer[1] = state + 48; // convert to char
		buffer[2] = '0';
		buffer[3] = '0';
		buffer[4] = '0';
		buffer[5] = '0';
		buffer[6] = '0';
		buffer[7] = '0';
		buffer[8] = '0';
		buffer[9] = '0';
		buffer[10] = '0';
		Arduino->WriteData(buffer,11);
	}
	void SendFire() // not yet implemented. waiting on motors
	{
		printf("Fireing Main Cannon!!!\n");
		buffer[0] = '3';
		buffer[1] = '0'; 
		buffer[2] = '0';
		buffer[3] = '0';
		buffer[4] = '0';
		buffer[5] = '0';
		buffer[6] = '0';
		buffer[7] = '0';
		buffer[8] = '0';
		buffer[9] = '0';
		buffer[10] = '0';
		Arduino->WriteData(buffer,11);
	}

	int isConnected()
	{
		return Arduino->IsConnected();
	}

};



class VirtualCoordinateProcessor
{
private:
	int targetX;
	int targetY;
	long virtualx;
	long virtualy;
	time_t lasttime;
	//int lasttime;
	float PWMX;
	float PWMY;
	float vAngle;
	float hAngle;
	int previousDistance;
	OutputHandler* MCH;


public:
	VirtualCoordinateProcessor(OutputHandler* temp)
	{
		//send initailazation coordinates
		time(&lasttime);
		hAngle = 60;
		vAngle = 60;
		virtualx = 90;
		virtualy = 90;
		PWMX = 1.5;
		PWMY = 1.5;
		//lasttime = clock();
		previousDistance = 0;
		MCH = temp;
	}
	bool set_Target(int x,int y)
	{
		float sendX;
		float sendY;
		float speed;
		float diff;
		long varianceX;
		long varianceY;
		time_t currentTime;
		int newdistancex;
		int newdistancey;
		int speedx;
		int speedy;
		int tempx;
		int tempy;
		//int currentTime;

		//send coordinates---using a basic linear mapping -> Coordinates to angles
		sendX = ((hAngle/639) * x) + 53; //this will need some calibration to find the limits of the cammera
		sendY = ((vAngle/479) * y) + 53; //same here
		printf("%f %f\n",sendX,sendY);
		/*PWMX = ((.33*sendX)/75) + 1.1368; //angles to PWM Signals
		PWMY = ((.36*sendY)/75) + .8356;*/
		PWMX = ((.33/638)* x) + 1.375;
		printf("FIRESHIT\n");
		PWMY = (((-7.536589e-4)*(y-50)) + 1.60);


		//printf("%f %f\n",PWMX,PWMY);
		//send the coordinates using the output handler
		MCH->SendPWM(PWMX,PWMY);



		/*
		time(&currentTime);
		diff = difftime(currentTime,lasttime);
		newdistancex = targetX - ((800/hAngle)*virtualx);
		newdistancey = targetY - ((600/vAngle)*virtualy);
		
		if(newdistancex < 0)
		{
			speedx = -333;

			if((speedx * diff) <= newdistancex)
			{
				// target reached
				virtualx = ((800/hAngle)*virtualx);
			}
			else
			{
				//target not reached
				virtualx = (speedx * diff);
			}
		}
		else
		{
			speedx = 333;
			if((speedx * diff) <= newdistancex)
			{
				//target not reached
				virtualx = (speedx * diff);
			}
			else
			{
				// target reached
				virtualx = ((800/hAngle)*virtualx);
			}
		}

		if(newdistancey < 0)
		{
			speedy = -333;

			if((speedy * diff) <= newdistancey)
			{
				// target reached
				virtualy = ((800/vAngle)*virtualy);
			}
			else
			{
				//target not reached
				virtualy = (speedy * diff);
			}
		}
		else
		{
			speedy = 333;
			if((speedy * diff) <= newdistancey)
			{
				//target not reached
				virtualy = (speedy * diff);
			}
			else
			{
				// target reached
				targetY = ((800/vAngle)*virtualy);
			}
		}
		
		
		if((abs(targetX - virtualx) + abs(targetY - virtualy)) < 100)
		{
			return true;
		}
		else
		{
			return false;
		}*/

		return false;

	}
};


class ExecutionLayer
{
private:
	VirtualCoordinateProcessor* VCP;
	OutputHandler* MCH;
public:
	ExecutionLayer()
	{
		MCH = new OutputHandler();
		VCP = new VirtualCoordinateProcessor(MCH);
	}

	bool setTarget(int x,int y)
	{
		return VCP->set_Target(x,y);
	}

	void Sendfire()
	{
		printf("Calling the real fireing function\n");
		MCH->SendFire();
	}

	void SendMode(int mode)
	{
		MCH->setMode(mode);
	}

	void SendLaser(int state)
	{
		MCH->SendLaserOnOff(state);
	}
};

/*
int main()
{
	int type;
	int x;
	int y;
	ExecutionLayer* Ex = new ExecutionLayer();
	do
	{
		cout << "Press 1 to send coord, 2 to fire,3 to change mode leds, and 4 to quit" << endl;
		cin >> type;
		if(type == 1)
		{
			cout << "Enter X: " << endl;
			cin >> x;
			cout << "Enter Y: " << endl;
			cin >> y;

			Ex->setTarget(x,y);
		}
		else if(type == 2)
		{
			Ex->Sendfire();
		}
		else if(type == 3)
		{
			cout << "Enter mode :" << endl;
			cin >> x;
			Ex->SendMode(x);
		}


	}while(type != 4);
	return 0;
}
*/